#include "ladarwidget.h"
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QProcess>
#include <QDebug>
#include <QMessageBox>
#include <QFileDialog>
#include <QInputDialog>
#include <QDateTime>
#include <QFile>
#include <QDir>
#include <QSerialPortInfo>
#include <QThread>
#include <QCloseEvent>
#include <cstdlib>
#include <signal.h>
#include <sys/types.h>
#include <dirent.h>
#include "qrviz.hpp"

LadarWidget::LadarWidget(QWidget *parent) : QWidget(parent)
{
    cmdList = new QStringList();

    // 检查 ROS 版本
    const char* rosDistro = std::getenv("ROS_DISTRO");
    const char* rosVersion = std::getenv("ROS_VERSION");

    if(rosVersion)
    {
        m_rosVersion = std::atoi(rosVersion);
    }

    qInfo() << "ROS_VERSION:" << m_rosVersion << " rosDistro:" << rosDistro;

    // 根据 ROS 版本设置雷达启动命令
    if(m_rosVersion == 1){
        cmdList->push_back("roslaunch rslidar_sdk start.launch");
        // 添加格式转换命令到雷达启动命令中
        cmdList->push_back("rosrun rslidar_to_velodyne rslidar_to_velodyne");
    }
    else{
        cmdList->push_back("ros2 launch rslidar_sdk start_launch.py");
        // ROS2 的格式转换命令可能需要调整
        cmdList->push_back("ros2 run rslidar_to_velodyne rslidar_to_velodyne");
    }

    // 初始化串口
    m_serial = new QSerialPort(this);
    connect(m_serial, &QSerialPort::errorOccurred, this, &LadarWidget::onSerialError);

    // 设置默认保存路径
    m_savePath = QDir::homePath() + "/rosbag";

    // 设置默认继电器设备
    m_relayDevice = "/dev/ttyACM0";

    setupUI();
    createConnections();
}

LadarWidget::~LadarWidget()
{
    closeLadar();
    closeImu();

    // 确保关闭继电器
    relayControl(1, "off");

    delete cmdList;
}

void LadarWidget::closeEvent(QCloseEvent *event)
{
    // 在窗口关闭时自动关闭继电器
    relayControl(1, "off");

    // 停止所有进程
    closeLadar();
    closeImu();

    event->accept();
}

void LadarWidget::setupUI()
{
    setContentsMargins(0,0,0,0);
    QVBoxLayout *p = new QVBoxLayout(this);
    p->setSpacing(0);
    p->setContentsMargins(0,0,0,0);

    QHBoxLayout *hL = new QHBoxLayout();

    m_btnOpenLader = new QPushButton(this);
    m_btnOpenLader->setText("打开雷达");
    m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
    m_btnOpenLader->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnOpenImu = new QPushButton(this);
    m_btnOpenImu->setText("打开IMU");
    m_btnOpenImu->setObjectName("closeLadar"); // 使用关闭雷达的样式
    m_btnOpenImu->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnShowLader = new QPushButton(this);
    m_btnShowLader->setText("显示雷达");
    m_btnShowLader->setObjectName("laderHide");
    m_btnShowLader->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnSavePath = new QPushButton(this);
    m_btnSavePath->setText("保存地址");
    m_btnSavePath->setCursor(QCursor(Qt::PointingHandCursor));

    m_btnRecord = new QPushButton(this);
    m_btnRecord->setText("记录");
    m_btnRecord->setCursor(QCursor(Qt::PointingHandCursor));

    hL->addStretch(1);
    hL->addWidget(m_btnOpenLader);
    hL->addSpacing(12);
    hL->addWidget(m_btnOpenImu);
    hL->addSpacing(12);
    hL->addWidget(m_btnShowLader);
    hL->addSpacing(12);
    hL->addWidget(m_btnSavePath);
    hL->addSpacing(12);
    hL->addWidget(m_btnRecord);
    hL->addStretch(1);

    // 主 RViz 显示
    m_rviz = new QRviz(this);
    m_rviz->setObjectName("rviz");
    m_rviz->setStyle(m_rviz->style());

    // 在主 RViz 中显示雷达点云
    m_rviz->Display_LaserScan(true, "/rslidar_points");

    // 单独的雷达显示窗口
    m_ladarRviz = new QRviz(this);
    m_ladarRviz->setObjectName("rvizLadar");
    m_ladarRviz->setFixedSize(1800, 900);
    m_ladarRviz->Display_Lader();  // 显示雷达点云
    m_ladarRviz->hide();

    this->setStyle(this->style());

    p->addWidget(m_rviz);
    p->addLayout(hL);
    p->addSpacing(6);
}

void LadarWidget::createConnections()
{
    connect(m_btnOpenLader, &QPushButton::clicked, this, &LadarWidget::onOpenLadarClicked);
    connect(m_btnOpenImu, &QPushButton::clicked, this, &LadarWidget::onOpenImuClicked);
    connect(m_btnShowLader, &QPushButton::clicked, this, &LadarWidget::onShowLadarImg);
    connect(m_btnSavePath, &QPushButton::clicked, this, &LadarWidget::onSavePathClicked);
    connect(m_btnRecord, &QPushButton::clicked, this, &LadarWidget::onRecordClicked);
}

// 计算校验码（数据1+数据2+数据3）
unsigned char LadarWidget::calculateChecksum(unsigned char data1, unsigned char data2, unsigned char data3)
{
    return (data1 + data2 + data3) & 0xFF;
}

// 检查设备是否被占用
bool LadarWidget::checkDeviceBusy(const QString& device)
{
    // 这里简化处理，实际可以使用系统调用检查设备占用情况
    // 在Linux下可以使用lsof命令，但这里我们只检查设备是否存在
    QFile file(device);
    return file.exists();
}

// 设置串口参数
bool LadarWidget::setupSerial(const QString& device)
{
    if (!m_serial->isOpen()) {
        m_serial->setPortName(device);
        m_serial->setBaudRate(QSerialPort::Baud115200);
        m_serial->setDataBits(QSerialPort::Data8);
        m_serial->setParity(QSerialPort::NoParity);
        m_serial->setStopBits(QSerialPort::OneStop);
        m_serial->setFlowControl(QSerialPort::NoFlowControl);

        if (m_serial->open(QIODevice::ReadWrite)) {
            qDebug() << "成功连接串口:" << device;
            return true;
        } else {
            qWarning() << "无法打开串口:" << device << m_serial->errorString();
            return false;
        }
    }
    return true;
}

// 继电器控制函数
void LadarWidget::relayControl(int relayNum, const QString& action)
{
    // 检查设备是否存在
    if (!checkDeviceBusy(m_relayDevice)) {
        qWarning() << "继电器设备不存在:" << m_relayDevice;
        return;
    }

    // 设置串口
    if (!setupSerial(m_relayDevice)) {
        qWarning() << "无法打开串口设备:" << m_relayDevice;
        return;
    }

    unsigned char data1 = 0xA0; // 固定头
    unsigned char data2 = relayNum; // 继电器编号
    unsigned char data3 = 0x00; // 数据3
    unsigned char checksum = 0x00; // 校验码

    // 根据操作设置数据3
    if (action == "on") {
        data3 = 0x01;
    } else if (action == "off") {
        data3 = 0x00;
    } else if (action == "status") {
        data3 = 0x02;
    } else {
        qWarning() << "未知的操作类型:" << action;
        return;
    }

    // 计算校验码
    checksum = calculateChecksum(data1, data2, data3);

    // 构建命令数据
    QByteArray command;
    command.append(static_cast<char>(data1));
    command.append(static_cast<char>(data2));
    command.append(static_cast<char>(data3));
    command.append(static_cast<char>(checksum));

    // 发送命令
    qint64 bytesWritten = m_serial->write(command);
    if (bytesWritten == -1) {
        qWarning() << "发送命令失败:" << m_serial->errorString();
    } else {
        m_serial->waitForBytesWritten(1000); // 等待数据写入
        qDebug() << "继电器命令已发送:" << action << "继电器" << relayNum;

        // 如果是查询状态，可以读取返回数据
        if (action == "status") {
            if (m_serial->waitForReadyRead(1000)) {
                QByteArray response = m_serial->readAll();
                qDebug() << "继电器状态响应:" << response.toHex();
            }
        }
    }
}

void LadarWidget::onOpenLadarClicked()
{
    if(m_ladarRunning) {
        // 停止雷达
        closeLadar();

        // 发送关闭继电器命令（控制第1路继电器）
        relayControl(1, "off");

        m_btnOpenLader->setText("打开雷达");
        m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
        m_ladarRunning = false;
        QMessageBox::information(this, "提示", "雷达已关闭");
    } else {
        // 先发送打开继电器命令（控制第1路继电器）
        relayControl(1, "on");

        // 等待继电器动作完成
        QThread::msleep(500);

        // 启动雷达ROS节点和格式转换
        foreach(const QString &cmd, *cmdList) {
            QStringList cmds = cmd.split(' ');
            if(cmds.empty()) {
                continue;
            }

            QProcess *process = new QProcess(this);

            connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
                    [process](int exitCode, QProcess::ExitStatus exitStatus) {
                qDebug() << process->program() << process->arguments() << process->pid()
                         << "节点退出，代码:" << exitCode << process->errorString();
                process->deleteLater();
            });

            QString firstCmd = cmd.trimmed();
            process->start(firstCmd);

            if (!process->waitForStarted()) {
                qWarning() << firstCmd << " " << cmds << " 启动失败." << process->errorString();
                process->deleteLater();
            } else {
                qDebug() << firstCmd << " " << cmds << process->processId() << "启动成功";
                m_processes.push_back(process);
                pidList.push_back(process->processId());
            }
        }

        m_btnOpenLader->setText("关闭雷达");
        m_btnOpenLader->setObjectName("openLadar"); // 使用打开雷达的样式
        m_ladarRunning = true;
        QMessageBox::information(this, "提示", "雷达和格式转换已启动");
    }

    m_btnOpenLader->setStyle(m_btnOpenLader->style());
}

void LadarWidget::onOpenImuClicked()
{
    if(m_imuRunning) {
        // 停止IMU
        closeImu();
        m_btnOpenImu->setText("打开IMU");
        m_btnOpenImu->setObjectName("closeLadar"); // 使用关闭雷达的样式
        m_imuRunning = false;
    } else {
        // 启动IMU节点
        m_imuProcess = new QProcess(this);

        connect(m_imuProcess, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
                [this](int exitCode, QProcess::ExitStatus exitStatus) {
            qDebug() << "IMU进程退出，代码:" << exitCode;
            m_imuRunning = false;
            m_btnOpenImu->setText("打开IMU");
            m_btnOpenImu->setObjectName("closeLadar");
            m_btnOpenImu->setStyle(m_btnOpenImu->style());
            m_imuProcess->deleteLater();
            m_imuProcess = nullptr;
        });

        QString cmd = "rosrun openzen_sensor openzen_sensor_node";
        m_imuProcess->start(cmd);

        if (!m_imuProcess->waitForStarted()) {
            qWarning() << cmd << " 启动失败." << m_imuProcess->errorString();
            QMessageBox::warning(this, "警告", "IMU进程启动失败: " + m_imuProcess->errorString());
            m_imuProcess->deleteLater();
            m_imuProcess = nullptr;
        } else {
            qDebug() << cmd << "启动成功";
            m_btnOpenImu->setText("关闭IMU");
            m_btnOpenImu->setObjectName("openLadar"); // 使用打开雷达的样式
            m_imuRunning = true;
            QMessageBox::information(this, "提示", "IMU已启动");
        }
    }

    m_btnOpenImu->setStyle(m_btnOpenImu->style());
}

void LadarWidget::onShowLadarImg()
{
    if(!m_laderRvizState) {
        m_ladarRviz->Show();
        m_ladarRviz->show();
        m_btnShowLader->setText("隐藏雷达");
        m_btnShowLader->setObjectName("laderShow");
    } else {
        m_ladarRviz->hide();
        m_ladarRviz->Hide();
        m_btnShowLader->setText("显示雷达");
        m_btnShowLader->setObjectName("laderHide");
    }

    m_laderRvizState = !m_laderRvizState;
    m_btnShowLader->setStyle(m_btnShowLader->style());
}

void LadarWidget::onSavePathClicked()
{
    QString path = QFileDialog::getExistingDirectory(this, "选择rosbag保存目录", m_savePath, QFileDialog::DontUseNativeDialog);
    if (path == "")
    {
        return;
    }

    m_savePath = path;
    QMessageBox::information(this, "提示", "保存路径已设置为: " + m_savePath);
    qDebug() << "保存路径设置为:" << m_savePath;
}

void LadarWidget::onRecordClicked()
{
    if (m_isRecording) {
        // 停止录制
        if (m_recordProcess && m_recordProcess->state() == QProcess::Running) {
            m_recordProcess->terminate();
            if (!m_recordProcess->waitForFinished(3000)) {
                m_recordProcess->kill();
            }

            // 录制完成后询问是否重命名
            bool ok;
            QString defaultName = "recorded_data_" + QDateTime::currentDateTime().toString("yyyyMMdd_hhmmss");
            QString newName = QInputDialog::getText(this, "重命名bag文件",
                                                   "请输入新的文件名（不含扩展名）:",
                                                   QLineEdit::Normal, defaultName, &ok);

            if (ok && !newName.isEmpty()) {
                renameBagFile(m_currentBagPath, newName);
            }

            m_recordProcess->deleteLater();
            m_recordProcess = nullptr;
        }

        m_btnRecord->setText("记录");
        m_isRecording = false;
        QMessageBox::information(this, "提示", "录制已停止，数据保存在: " + m_savePath);
    } else {
        // 开始录制
        if (m_savePath.isEmpty()) {
            QMessageBox::warning(this, "警告", "请先设置保存路径");
            return;
        }

        // 确保目录存在
        QDir dir;
        if (!dir.exists(m_savePath)) {
            dir.mkpath(m_savePath);
        }

        m_recordProcess = new QProcess(this);

        connect(m_recordProcess, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
                [this](int exitCode, QProcess::ExitStatus exitStatus) {
            qDebug() << "录制进程退出，代码:" << exitCode;
            m_isRecording = false;
            m_btnRecord->setText("记录");
            m_recordProcess->deleteLater();
            m_recordProcess = nullptr;
        });

        // 根据ROS版本使用不同的录制命令
        QString cmd;
        QString bagFileName;
        if (m_rosVersion == 1) {
            // ROS1录制命令 - 录制两个话题
            bagFileName = "recorded_data";
            cmd = QString("rosbag record -O %1/%2 /velodyne_points /imu/data").arg(m_savePath).arg(bagFileName);
            m_currentBagPath = m_savePath + "/" + bagFileName + ".bag";
        } else {
            // ROS2录制命令 - 录制两个话题
            bagFileName = "recorded_data";
            cmd = QString("ros2 bag record -o %1/%2 /velodyne_points /imu/data").arg(m_savePath).arg(bagFileName);
            m_currentBagPath = m_savePath + "/" + bagFileName;
        }

        m_recordProcess->start(cmd);

        if (!m_recordProcess->waitForStarted()) {
            qWarning() << cmd << " 启动失败." << m_recordProcess->errorString();
            QMessageBox::warning(this, "警告", "录制进程启动失败: " + m_recordProcess->errorString());
            m_recordProcess->deleteLater();
            m_recordProcess = nullptr;
        } else {
            qDebug() << cmd << "启动成功";
            m_btnRecord->setText("停止记录");
            m_isRecording = true;
            QMessageBox::information(this, "提示", "开始录制 /velodyne_points 和 /imu/data 话题数据");
        }
    }
}

void LadarWidget::closeLadar()
{
    // 如果正在录制，先停止录制
    if (m_isRecording && m_recordProcess) {
        m_recordProcess->terminate();
        m_recordProcess->waitForFinished(1000);
        m_recordProcess->deleteLater();
        m_recordProcess = nullptr;
        m_isRecording = false;
        m_btnRecord->setText("记录");
    }

    // 杀死子进程
    QVector<quint64> vecChildId;
    for(int i =0; i < pidList.size(); i++)
    {
       get_child_pids(pidList[i],vecChildId);
    }

    foreach(auto pid, vecChildId)
    {
        qDebug() << "try to kill pid" << pid;
        kill_process(pid);
    }

    // 停止所有进程
    foreach(QProcess *process, m_processes) {
        if (process) {
            if (!(process->state() == QProcess::NotRunning)) {
                process->terminate();
                process->waitForFinished(1000);
            }
            process->deleteLater();
        }
    }

    m_processes.clear();
    pidList.clear();
    m_ladarRunning = false;
}

void LadarWidget::closeImu()
{
    if (m_imuProcess && m_imuProcess->state() == QProcess::Running) {
        m_imuProcess->terminate();
        if (!m_imuProcess->waitForFinished(3000)) {
            m_imuProcess->kill();
        }
        m_imuProcess->deleteLater();
        m_imuProcess = nullptr;
    }
    m_imuRunning = false;
}

void LadarWidget::renameBagFile(const QString& oldPath, const QString& newName)
{
    QFileInfo fileInfo(oldPath);
    QString extension = fileInfo.suffix(); // 获取文件扩展名
    QString dirPath = fileInfo.dir().path();

    QString newPath;
    if (m_rosVersion == 1) {
        // ROS1: 文件格式为 .bag
        newPath = dirPath + "/" + newName + ".bag";
    } else {
        // ROS2: 目录格式，没有扩展名
        newPath = dirPath + "/" + newName;
    }

    QFile file(oldPath);
    if (file.exists()) {
        if (file.rename(newPath)) {
            QMessageBox::information(this, "成功", QString("bag文件已重命名为: %1").arg(newName));
            qDebug() << "bag文件重命名成功:" << oldPath << "->" << newPath;
        } else {
            QMessageBox::warning(this, "警告", QString("重命名失败: %1").arg(file.errorString()));
            qWarning() << "bag文件重命名失败:" << oldPath << "->" << newPath << file.errorString();
        }
    } else {
        // 对于ROS2，可能是目录
        QDir dir(oldPath);
        if (dir.exists()) {
            if (dir.rename(oldPath, newPath)) {
                QMessageBox::information(this, "成功", QString("bag文件已重命名为: %1").arg(newName));
                qDebug() << "bag目录重命名成功:" << oldPath << "->" << newPath;
            } else {
                QMessageBox::warning(this, "警告", "重命名目录失败");
                qWarning() << "bag目录重命名失败:" << oldPath << "->" << newPath;
            }
        } else {
            QMessageBox::warning(this, "警告", "找不到录制的bag文件");
            qWarning() << "找不到录制的bag文件:" << oldPath;
        }
    }
}

void LadarWidget::onLadarProcessFinished(int exitCode, QProcess::ExitStatus exitStatus)
{
    qDebug() << "雷达进程结束，退出码:" << exitCode;

    if (m_ladarRunning) {
        m_btnOpenLader->setText("打开雷达");
        m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
        m_btnOpenLader->setStyle(m_btnOpenLader->style());
        m_ladarRunning = false;
    }
}

void LadarWidget::onLadarProcessError(QProcess::ProcessError error)
{
    qWarning() << "雷达进程错误:" << error;

    QString errorString;
    switch (error) {
    case QProcess::FailedToStart:
        errorString = "进程无法启动";
        break;
    case QProcess::Crashed:
        errorString = "进程崩溃";
        break;
    case QProcess::Timedout:
        errorString = "进程超时";
        break;
    case QProcess::WriteError:
        errorString = "写入错误";
        break;
    case QProcess::ReadError:
        errorString = "读取错误";
        break;
    case QProcess::UnknownError:
    default:
        errorString = "未知错误";
        break;
    }

    QMessageBox::critical(this, "错误", "雷达进程错误: " + errorString);

    if (m_ladarRunning) {
        m_btnOpenLader->setText("打开雷达");
        m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
        m_btnOpenLader->setStyle(m_btnOpenLader->style());
        m_ladarRunning = false;
    }
}

void LadarWidget::onSerialError(QSerialPort::SerialPortError error)
{
    switch (error) {
    case QSerialPort::OpenError:
        qWarning() << "打开设备时出错";
        break;
    case QSerialPort::DeviceNotFoundError:
        qWarning() << "找不到设备";
        break;
    case QSerialPort::PermissionError:
        qWarning() << "没有足够的权限,请检查设备连接状态";
        break;
    case QSerialPort::ResourceError:
        qWarning() << "设备被意外移除或系统资源不足";
        break;
    case QSerialPort::WriteError:
        qWarning() << "写数据时出错";
        break;
    case QSerialPort::ReadError:
        qWarning() << "读数据时出错";
        break;
    default:
        if(error != QSerialPort::NoError){
            qWarning() << "串口错误，请检查设备";
        }
    }

    if(error != QSerialPort::NoError) {
        if(m_serial->isOpen()) {
            m_serial->close();
        }
    }
}

void LadarWidget::kill_process(quint64 pid)
{
    if(pid <= 0) return;

    int ret = kill(pid, SIGTERM);
    if(ret == -1) {
        qDebug() << "kill SIGTERM failed for pid" << pid;
        kill(pid, SIGKILL);
    }
}

void LadarWidget::get_child_pids(quint64 parentPid, QVector<quint64> &childPids)
{
    DIR *dir;
    struct dirent *entry;
    char path[256];
    FILE *file;
    char line[256];
    quint64 pid, ppid;

    if ((dir = opendir("/proc")) == NULL) {
        return;
    }

    while ((entry = readdir(dir)) != NULL) {
        if (entry->d_type != DT_DIR) continue;

        char *endptr;
        pid = strtoul(entry->d_name, &endptr, 10);
        if (*endptr != '\0') continue;

        snprintf(path, sizeof(path), "/proc/%lu/stat", pid);
        file = fopen(path, "r");
        if (file == NULL) continue;

        if (fgets(line, sizeof(line), file)) {
            char *token = strtok(line, " ");
            for (int i = 1; i < 4; i++) {
                token = strtok(NULL, " ");
                if (i == 3) { // ppid is the 4th field
                    ppid = strtoul(token, NULL, 10);
                    if (ppid == parentPid) {
                        childPids.push_back(pid);
                    }
                    break;
                }
            }
        }
        fclose(file);
    }
    closedir(dir);
}




















//#include "ladarwidget.h"
//#include <QHBoxLayout>
//#include <QVBoxLayout>
//#include <QProcess>
//#include <QDebug>
//#include <QMessageBox>
//#include <QFileDialog>
//#include <QInputDialog>
//#include <QDateTime>
//#include <QFile>
//#include <QDir>
//#include <QSerialPortInfo>
//#include <QThread>
//#include <cstdlib>
//#include <signal.h>
//#include <sys/types.h>
//#include <dirent.h>
//#include "qrviz.hpp"

//LadarWidget::LadarWidget(QWidget *parent) : QWidget(parent)
//{
//    cmdList = new QStringList();

//    // 检查 ROS 版本
//    const char* rosDistro = std::getenv("ROS_DISTRO");
//    const char* rosVersion = std::getenv("ROS_VERSION");

//    if(rosVersion)
//    {
//        m_rosVersion = std::atoi(rosVersion);
//    }

//    qInfo() << "ROS_VERSION:" << m_rosVersion << " rosDistro:" << rosDistro;

//    // 根据 ROS 版本设置雷达启动命令
//    if(m_rosVersion == 1){
//        cmdList->push_back("roslaunch rslidar_sdk start.launch");
//        // 添加格式转换命令到雷达启动命令中
//        cmdList->push_back("rosrun rslidar_to_velodyne rslidar_to_velodyne");
//    }
//    else{
//        cmdList->push_back("ros2 launch rslidar_sdk start_launch.py");
//        // ROS2 的格式转换命令可能需要调整
//        cmdList->push_back("ros2 run rslidar_to_velodyne rslidar_to_velodyne");
//    }

//    // 初始化串口
//    m_serial = new QSerialPort(this);
//    connect(m_serial, &QSerialPort::errorOccurred, this, &LadarWidget::onSerialError);

//    // 设置默认保存路径
//    m_savePath = QDir::homePath() + "/rosbag";

//    // 设置默认继电器设备
//    m_relayDevice = "/dev/ttyACM0";

//    setupUI();
//    createConnections();
//}

//LadarWidget::~LadarWidget()
//{
//    closeLadar();
//    closeImu();
//    delete cmdList;
//}

//void LadarWidget::setupUI()
//{
//    setContentsMargins(0,0,0,0);
//    QVBoxLayout *p = new QVBoxLayout(this);
//    p->setSpacing(0);
//    p->setContentsMargins(0,0,0,0);

//    QHBoxLayout *hL = new QHBoxLayout();

//    m_btnOpenLader = new QPushButton(this);
//    m_btnOpenLader->setText("打开雷达");
//    m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
//    m_btnOpenLader->setCursor(QCursor(Qt::PointingHandCursor));

//    m_btnOpenImu = new QPushButton(this);
//    m_btnOpenImu->setText("打开IMU");
//    m_btnOpenImu->setObjectName("closeLadar"); // 使用关闭雷达的样式
//    m_btnOpenImu->setCursor(QCursor(Qt::PointingHandCursor));

//    m_btnShowLader = new QPushButton(this);
//    m_btnShowLader->setText("显示雷达");
//    m_btnShowLader->setObjectName("laderHide");
//    m_btnShowLader->setCursor(QCursor(Qt::PointingHandCursor));

//    m_btnSavePath = new QPushButton(this);
//    m_btnSavePath->setText("保存地址");
//    m_btnSavePath->setCursor(QCursor(Qt::PointingHandCursor));

//    m_btnRecord = new QPushButton(this);
//    m_btnRecord->setText("记录");
//    m_btnRecord->setCursor(QCursor(Qt::PointingHandCursor));

//    hL->addStretch(1);
//    hL->addWidget(m_btnOpenLader);
//    hL->addSpacing(12);
//    hL->addWidget(m_btnOpenImu);
//    hL->addSpacing(12);
//    hL->addWidget(m_btnShowLader);
//    hL->addSpacing(12);
//    hL->addWidget(m_btnSavePath);
//    hL->addSpacing(12);
//    hL->addWidget(m_btnRecord);
//    hL->addStretch(1);

//    // 主 RViz 显示
//    m_rviz = new QRviz(this);
//    m_rviz->setObjectName("rviz");
//    m_rviz->setStyle(m_rviz->style());

//    // 在主 RViz 中显示雷达点云
//    m_rviz->Display_LaserScan(true, "/rslidar_points");

//    // 单独的雷达显示窗口
//    m_ladarRviz = new QRviz(this);
//    m_ladarRviz->setObjectName("rvizLadar");
//    m_ladarRviz->setFixedSize(1800, 900);
//    m_ladarRviz->Display_Lader();  // 显示雷达点云
//    m_ladarRviz->hide();

//    this->setStyle(this->style());

//    p->addWidget(m_rviz);
//    p->addLayout(hL);
//    p->addSpacing(6);
//}

//void LadarWidget::createConnections()
//{
//    connect(m_btnOpenLader, &QPushButton::clicked, this, &LadarWidget::onOpenLadarClicked);
//    connect(m_btnOpenImu, &QPushButton::clicked, this, &LadarWidget::onOpenImuClicked);
//    connect(m_btnShowLader, &QPushButton::clicked, this, &LadarWidget::onShowLadarImg);
//    connect(m_btnSavePath, &QPushButton::clicked, this, &LadarWidget::onSavePathClicked);
//    connect(m_btnRecord, &QPushButton::clicked, this, &LadarWidget::onRecordClicked);
//}

//// 计算校验码（数据1+数据2+数据3）
//unsigned char LadarWidget::calculateChecksum(unsigned char data1, unsigned char data2, unsigned char data3)
//{
//    return (data1 + data2 + data3) & 0xFF;
//}

//// 检查设备是否被占用
//bool LadarWidget::checkDeviceBusy(const QString& device)
//{
//    // 这里简化处理，实际可以使用系统调用检查设备占用情况
//    // 在Linux下可以使用lsof命令，但这里我们只检查设备是否存在
//    QFile file(device);
//    return file.exists();
//}

//// 设置串口参数
//bool LadarWidget::setupSerial(const QString& device)
//{
//    if (!m_serial->isOpen()) {
//        m_serial->setPortName(device);
//        m_serial->setBaudRate(QSerialPort::Baud115200);
//        m_serial->setDataBits(QSerialPort::Data8);
//        m_serial->setParity(QSerialPort::NoParity);
//        m_serial->setStopBits(QSerialPort::OneStop);
//        m_serial->setFlowControl(QSerialPort::NoFlowControl);

//        if (m_serial->open(QIODevice::ReadWrite)) {
//            qDebug() << "成功连接串口:" << device;
//            return true;
//        } else {
//            qWarning() << "无法打开串口:" << device << m_serial->errorString();
//            return false;
//        }
//    }
//    return true;
//}

//// 继电器控制函数
//void LadarWidget::relayControl(int relayNum, const QString& action)
//{
//    // 检查设备是否存在
//    if (!checkDeviceBusy(m_relayDevice)) {
//        QMessageBox::warning(this, "警告", QString("继电器设备 %1 不存在").arg(m_relayDevice));
//        return;
//    }

//    // 设置串口
//    if (!setupSerial(m_relayDevice)) {
//        QMessageBox::warning(this, "警告", "无法打开串口设备");
//        return;
//    }

//    unsigned char data1 = 0xA0; // 固定头
//    unsigned char data2 = relayNum; // 继电器编号
//    unsigned char data3 = 0x00; // 数据3
//    unsigned char checksum = 0x00; // 校验码

//    // 根据操作设置数据3
//    if (action == "on") {
//        data3 = 0x01;
//    } else if (action == "off") {
//        data3 = 0x00;
//    } else if (action == "status") {
//        data3 = 0x02;
//    } else {
//        QMessageBox::warning(this, "警告", "未知的操作类型");
//        return;
//    }

//    // 计算校验码
//    checksum = calculateChecksum(data1, data2, data3);

//    // 构建命令数据
//    QByteArray command;
//    command.append(static_cast<char>(data1));
//    command.append(static_cast<char>(data2));
//    command.append(static_cast<char>(data3));
//    command.append(static_cast<char>(checksum));

//    // 发送命令
//    qint64 bytesWritten = m_serial->write(command);
//    if (bytesWritten == -1) {
//        QMessageBox::warning(this, "警告", "发送命令失败: " + m_serial->errorString());
//    } else {
//        m_serial->waitForBytesWritten(1000); // 等待数据写入
//        qDebug() << "继电器命令已发送:" << action << "继电器" << relayNum;

//        // 如果是查询状态，可以读取返回数据
//        if (action == "status") {
//            if (m_serial->waitForReadyRead(1000)) {
//                QByteArray response = m_serial->readAll();
//                qDebug() << "继电器状态响应:" << response.toHex();
//            }
//        }
//    }
//}

//void LadarWidget::onOpenLadarClicked()
//{
//    if(m_ladarRunning) {
//        // 停止雷达
//        closeLadar();

//        // 发送关闭继电器命令（控制第1路继电器）
//        relayControl(1, "off");

//        m_btnOpenLader->setText("打开雷达");
//        m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
//        m_ladarRunning = false;
//        QMessageBox::information(this, "提示", "雷达已关闭");
//    } else {
//        // 先发送打开继电器命令（控制第1路继电器）
//        relayControl(1, "on");

//        // 等待继电器动作完成
//        QThread::msleep(500);

//        // 启动雷达ROS节点和格式转换
//        foreach(const QString &cmd, *cmdList) {
//            QStringList cmds = cmd.split(' ');
//            if(cmds.empty()) {
//                continue;
//            }

//            QProcess *process = new QProcess(this);

//            connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
//                    [process](int exitCode, QProcess::ExitStatus exitStatus) {
//                qDebug() << process->program() << process->arguments() << process->pid()
//                         << "节点退出，代码:" << exitCode << process->errorString();
//                process->deleteLater();
//            });

//            QString firstCmd = cmd.trimmed();
//            process->start(firstCmd);

//            if (!process->waitForStarted()) {
//                qWarning() << firstCmd << " " << cmds << " 启动失败." << process->errorString();
//                process->deleteLater();
//            } else {
//                qDebug() << firstCmd << " " << cmds << process->processId() << "启动成功";
//                m_processes.push_back(process);
//                pidList.push_back(process->processId());
//            }
//        }

//        m_btnOpenLader->setText("关闭雷达");
//        m_btnOpenLader->setObjectName("openLadar"); // 使用打开雷达的样式
//        m_ladarRunning = true;
//        QMessageBox::information(this, "提示", "雷达和格式转换已启动");
//    }

//    m_btnOpenLader->setStyle(m_btnOpenLader->style());
//}

//void LadarWidget::onOpenImuClicked()
//{
//    if(m_imuRunning) {
//        // 停止IMU
//        closeImu();
//        m_btnOpenImu->setText("打开IMU");
//        m_btnOpenImu->setObjectName("closeLadar"); // 使用关闭雷达的样式
//        m_imuRunning = false;
//    } else {
//        // 启动IMU节点
//        m_imuProcess = new QProcess(this);

//        connect(m_imuProcess, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
//                [this](int exitCode, QProcess::ExitStatus exitStatus) {
//            qDebug() << "IMU进程退出，代码:" << exitCode;
//            m_imuRunning = false;
//            m_btnOpenImu->setText("打开IMU");
//            m_btnOpenImu->setObjectName("closeLadar");
//            m_btnOpenImu->setStyle(m_btnOpenImu->style());
//            m_imuProcess->deleteLater();
//            m_imuProcess = nullptr;
//        });

//        QString cmd = "rosrun openzen_sensor openzen_sensor_node";
//        m_imuProcess->start(cmd);

//        if (!m_imuProcess->waitForStarted()) {
//            qWarning() << cmd << " 启动失败." << m_imuProcess->errorString();
//            QMessageBox::warning(this, "警告", "IMU进程启动失败: " + m_imuProcess->errorString());
//            m_imuProcess->deleteLater();
//            m_imuProcess = nullptr;
//        } else {
//            qDebug() << cmd << "启动成功";
//            m_btnOpenImu->setText("关闭IMU");
//            m_btnOpenImu->setObjectName("openLadar"); // 使用打开雷达的样式
//            m_imuRunning = true;
//            QMessageBox::information(this, "提示", "IMU已启动");
//        }
//    }

//    m_btnOpenImu->setStyle(m_btnOpenImu->style());
//}

//void LadarWidget::onShowLadarImg()
//{
//    if(!m_laderRvizState) {
//        m_ladarRviz->Show();
//        m_ladarRviz->show();
//        m_btnShowLader->setText("隐藏雷达");
//        m_btnShowLader->setObjectName("laderShow");
//    } else {
//        m_ladarRviz->hide();
//        m_ladarRviz->Hide();
//        m_btnShowLader->setText("显示雷达");
//        m_btnShowLader->setObjectName("laderHide");
//    }

//    m_laderRvizState = !m_laderRvizState;
//    m_btnShowLader->setStyle(m_btnShowLader->style());
//}

//void LadarWidget::onSavePathClicked()
//{
//    QString path = QFileDialog::getExistingDirectory(this, "选择rosbag保存目录", m_savePath, QFileDialog::DontUseNativeDialog);
//    if (path == "")
//    {
//        return;
//    }

//    m_savePath = path;
//    QMessageBox::information(this, "提示", "保存路径已设置为: " + m_savePath);
//    qDebug() << "保存路径设置为:" << m_savePath;
//}

//void LadarWidget::onRecordClicked()
//{
//    if (m_isRecording) {
//        // 停止录制
//        if (m_recordProcess && m_recordProcess->state() == QProcess::Running) {
//            m_recordProcess->terminate();
//            if (!m_recordProcess->waitForFinished(3000)) {
//                m_recordProcess->kill();
//            }

//            // 录制完成后询问是否重命名
//            bool ok;
//            QString defaultName = "recorded_data_" + QDateTime::currentDateTime().toString("yyyyMMdd_hhmmss");
//            QString newName = QInputDialog::getText(this, "重命名bag文件",
//                                                   "请输入新的文件名（不含扩展名）:",
//                                                   QLineEdit::Normal, defaultName, &ok);

//            if (ok && !newName.isEmpty()) {
//                renameBagFile(m_currentBagPath, newName);
//            }

//            m_recordProcess->deleteLater();
//            m_recordProcess = nullptr;
//        }

//        m_btnRecord->setText("记录");
//        m_isRecording = false;
//        QMessageBox::information(this, "提示", "录制已停止，数据保存在: " + m_savePath);
//    } else {
//        // 开始录制
//        if (m_savePath.isEmpty()) {
//            QMessageBox::warning(this, "警告", "请先设置保存路径");
//            return;
//        }

//        // 确保目录存在
//        QDir dir;
//        if (!dir.exists(m_savePath)) {
//            dir.mkpath(m_savePath);
//        }

//        m_recordProcess = new QProcess(this);

//        connect(m_recordProcess, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
//                [this](int exitCode, QProcess::ExitStatus exitStatus) {
//            qDebug() << "录制进程退出，代码:" << exitCode;
//            m_isRecording = false;
//            m_btnRecord->setText("记录");
//            m_recordProcess->deleteLater();
//            m_recordProcess = nullptr;
//        });

//        // 根据ROS版本使用不同的录制命令
//        QString cmd;
//        QString bagFileName;
//        if (m_rosVersion == 1) {
//            // ROS1录制命令 - 录制两个话题
//            bagFileName = "recorded_data";
//            cmd = QString("rosbag record -O %1/%2 /velodyne_points /imu/data").arg(m_savePath).arg(bagFileName);
//            m_currentBagPath = m_savePath + "/" + bagFileName + ".bag";
//        } else {
//            // ROS2录制命令 - 录制两个话题
//            bagFileName = "recorded_data";
//            cmd = QString("ros2 bag record -o %1/%2 /velodyne_points /imu/data").arg(m_savePath).arg(bagFileName);
//            m_currentBagPath = m_savePath + "/" + bagFileName;
//        }

//        m_recordProcess->start(cmd);

//        if (!m_recordProcess->waitForStarted()) {
//            qWarning() << cmd << " 启动失败." << m_recordProcess->errorString();
//            QMessageBox::warning(this, "警告", "录制进程启动失败: " + m_recordProcess->errorString());
//            m_recordProcess->deleteLater();
//            m_recordProcess = nullptr;
//        } else {
//            qDebug() << cmd << "启动成功";
//            m_btnRecord->setText("停止记录");
//            m_isRecording = true;
//            QMessageBox::information(this, "提示", "开始录制 /velodyne_points 和 /imu/data 话题数据");
//        }
//    }
//}

//void LadarWidget::closeLadar()
//{
//    // 如果正在录制，先停止录制
//    if (m_isRecording && m_recordProcess) {
//        m_recordProcess->terminate();
//        m_recordProcess->waitForFinished(1000);
//        m_recordProcess->deleteLater();
//        m_recordProcess = nullptr;
//        m_isRecording = false;
//        m_btnRecord->setText("记录");
//    }

//    // 杀死子进程
//    QVector<quint64> vecChildId;
//    for(int i =0; i < pidList.size(); i++)
//    {
//       get_child_pids(pidList[i],vecChildId);
//    }

//    foreach(auto pid, vecChildId)
//    {
//        qDebug() << "try to kill pid" << pid;
//        kill_process(pid);
//    }

//    // 停止所有进程
//    foreach(QProcess *process, m_processes) {
//        if (process) {
//            if (!(process->state() == QProcess::NotRunning)) {
//                process->terminate();
//                process->waitForFinished(1000);
//            }
//            process->deleteLater();
//        }
//    }

//    m_processes.clear();
//    pidList.clear();
//    m_ladarRunning = false;
//}

//void LadarWidget::closeImu()
//{
//    if (m_imuProcess && m_imuProcess->state() == QProcess::Running) {
//        m_imuProcess->terminate();
//        if (!m_imuProcess->waitForFinished(3000)) {
//            m_imuProcess->kill();
//        }
//        m_imuProcess->deleteLater();
//        m_imuProcess = nullptr;
//    }
//    m_imuRunning = false;
//}

//void LadarWidget::renameBagFile(const QString& oldPath, const QString& newName)
//{
//    QFileInfo fileInfo(oldPath);
//    QString extension = fileInfo.suffix(); // 获取文件扩展名
//    QString dirPath = fileInfo.dir().path();

//    QString newPath;
//    if (m_rosVersion == 1) {
//        // ROS1: 文件格式为 .bag
//        newPath = dirPath + "/" + newName + ".bag";
//    } else {
//        // ROS2: 目录格式，没有扩展名
//        newPath = dirPath + "/" + newName;
//    }

//    QFile file(oldPath);
//    if (file.exists()) {
//        if (file.rename(newPath)) {
//            QMessageBox::information(this, "成功", QString("bag文件已重命名为: %1").arg(newName));
//            qDebug() << "bag文件重命名成功:" << oldPath << "->" << newPath;
//        } else {
//            QMessageBox::warning(this, "警告", QString("重命名失败: %1").arg(file.errorString()));
//            qWarning() << "bag文件重命名失败:" << oldPath << "->" << newPath << file.errorString();
//        }
//    } else {
//        // 对于ROS2，可能是目录
//        QDir dir(oldPath);
//        if (dir.exists()) {
//            if (dir.rename(oldPath, newPath)) {
//                QMessageBox::information(this, "成功", QString("bag文件已重命名为: %1").arg(newName));
//                qDebug() << "bag目录重命名成功:" << oldPath << "->" << newPath;
//            } else {
//                QMessageBox::warning(this, "警告", "重命名目录失败");
//                qWarning() << "bag目录重命名失败:" << oldPath << "->" << newPath;
//            }
//        } else {
//            QMessageBox::warning(this, "警告", "找不到录制的bag文件");
//            qWarning() << "找不到录制的bag文件:" << oldPath;
//        }
//    }
//}

//void LadarWidget::onLadarProcessFinished(int exitCode, QProcess::ExitStatus exitStatus)
//{
//    qDebug() << "雷达进程结束，退出码:" << exitCode;

//    if (m_ladarRunning) {
//        m_btnOpenLader->setText("打开雷达");
//        m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
//        m_btnOpenLader->setStyle(m_btnOpenLader->style());
//        m_ladarRunning = false;
//    }
//}

//void LadarWidget::onLadarProcessError(QProcess::ProcessError error)
//{
//    qWarning() << "雷达进程错误:" << error;

//    QString errorString;
//    switch (error) {
//    case QProcess::FailedToStart:
//        errorString = "进程无法启动";
//        break;
//    case QProcess::Crashed:
//        errorString = "进程崩溃";
//        break;
//    case QProcess::Timedout:
//        errorString = "进程超时";
//        break;
//    case QProcess::WriteError:
//        errorString = "写入错误";
//        break;
//    case QProcess::ReadError:
//        errorString = "读取错误";
//        break;
//    case QProcess::UnknownError:
//    default:
//        errorString = "未知错误";
//        break;
//    }

//    QMessageBox::critical(this, "错误", "雷达进程错误: " + errorString);

//    if (m_ladarRunning) {
//        m_btnOpenLader->setText("打开雷达");
//        m_btnOpenLader->setObjectName("closeLadar"); // 使用关闭雷达的样式
//        m_btnOpenLader->setStyle(m_btnOpenLader->style());
//        m_ladarRunning = false;
//    }
//}

//void LadarWidget::onSerialError(QSerialPort::SerialPortError error)
//{
//    switch (error) {
//    case QSerialPort::OpenError:
//        qWarning() << "打开设备时出错";
//        break;
//    case QSerialPort::DeviceNotFoundError:
//        qWarning() << "找不到设备";
//        break;
//    case QSerialPort::PermissionError:
//        qWarning() << "没有足够的权限,请检查设备连接状态";
//        break;
//    case QSerialPort::ResourceError:
//        qWarning() << "设备被意外移除或系统资源不足";
//        break;
//    case QSerialPort::WriteError:
//        qWarning() << "写数据时出错";
//        break;
//    case QSerialPort::ReadError:
//        qWarning() << "读数据时出错";
//        break;
//    default:
//        if(error != QSerialPort::NoError){
//            qWarning() << "串口错误，请检查设备";
//        }
//    }

//    if(error != QSerialPort::NoError) {
//        if(m_serial->isOpen()) {
//            m_serial->close();
//        }
//    }
//}

//void LadarWidget::kill_process(quint64 pid)
//{
//    if(pid <= 0) return;

//    int ret = kill(pid, SIGTERM);
//    if(ret == -1) {
//        qDebug() << "kill SIGTERM failed for pid" << pid;
//        kill(pid, SIGKILL);
//    }
//}

//void LadarWidget::get_child_pids(quint64 parentPid, QVector<quint64> &childPids)
//{
//    DIR *dir;
//    struct dirent *entry;
//    char path[256];
//    FILE *file;
//    char line[256];
//    quint64 pid, ppid;

//    if ((dir = opendir("/proc")) == NULL) {
//        return;
//    }

//    while ((entry = readdir(dir)) != NULL) {
//        if (entry->d_type != DT_DIR) continue;

//        char *endptr;
//        pid = strtoul(entry->d_name, &endptr, 10);
//        if (*endptr != '\0') continue;

//        snprintf(path, sizeof(path), "/proc/%lu/stat", pid);
//        file = fopen(path, "r");
//        if (file == NULL) continue;

//        if (fgets(line, sizeof(line), file)) {
//            char *token = strtok(line, " ");
//            for (int i = 1; i < 4; i++) {
//                token = strtok(NULL, " ");
//                if (i == 3) { // ppid is the 4th field
//                    ppid = strtoul(token, NULL, 10);
//                    if (ppid == parentPid) {
//                        childPids.push_back(pid);
//                    }
//                    break;
//                }
//            }
//        }
//        fclose(file);
//    }
//    closedir(dir);
//}
